#include "Motor.h"

MotorControl::MotorControl() {
}

void MotorControl::init() {
  Serial.println("电机初始化");
  pinMode(MOTOR_A1_PIN, OUTPUT);
  pinMode(MOTOR_A2_PIN, OUTPUT);
  pinMode(MOTOR_B1_PIN, OUTPUT);
  pinMode(MOTOR_B2_PIN, OUTPUT);
  pinMode(MOTOR_C1_PIN, OUTPUT);
  pinMode(MOTOR_C2_PIN, OUTPUT);
  pinMode(MOTOR_D1_PIN, OUTPUT);
  pinMode(MOTOR_D2_PIN, OUTPUT);

  Serial.println("舵机初始化");
  servo1.attach(SERVO_1_PIN);
  servo2.attach(SERVO_2_PIN);
  servo3.attach(SERVO_3_PIN);

  Serial.println("NRF初始化");
  nrf.init(0, RF24_PA_MIN);
}

// 读取收到的数据并写入tank_kvs
void MotorControl::read() {
  if (nrf.radio.available()) {
    nrf.radio.read(&tank_kvs, sizeof(tank_kvs));
  }
}

// 控制舵机
void MotorControl::loadServo() {
  if (tank_kvs.R_up == LOW) {
    servo1.write(_angle1);
    servo2.write(_angle2);
    servo3.write(_angle3);
  } else {
    int a1 = map(tank_kvs.RX, -100, 100, 25, 100);
    int a2 = map(tank_kvs.RY, -100, 100, 0, 80);
    int a3 = map(tank_kvs.RY, -100, 100, 0, 80);
    servo1.write(a1);
    servo2.write(a2);
    servo3.write(a3);

    // Serial.print("a1: ");
    // Serial.print(a1);
    // Serial.print("\t a2: ");
    // Serial.print(a2);
    // Serial.print("\t a3: ");
    // Serial.print(a3);
    // Serial.print("\t LX: ");
    // Serial.print(tank_kvs.LX);
    // Serial.print("\t LY: ");
    // Serial.print(tank_kvs.LY);
    // Serial.print("\t RX: ");
    // Serial.print(tank_kvs.RX);
    // Serial.print("\t RY: ");
    // Serial.println(tank_kvs.RY);
  }
}

// 控制电机
void MotorControl::loadMotor() {
  if (tank_kvs.LY < -15 && tank_kvs.LX > -30 && tank_kvs.LX < 30) {
    Serial.println("forward");
    clearSw();
    forward();
  } else if (tank_kvs.LY > 15 && tank_kvs.LX > -30 && tank_kvs.LX < 30) {
    Serial.println("backward");
    clearSw();
    backward();
  } else if (tank_kvs.LX < -15) {
    Serial.println("leftRotate");
    left_sw = true;
    leftRotate();
  } else if (tank_kvs.LX > 15) {
    Serial.println("rightRotate");
    right_sw = true;
    rightRotate();
  } else if (tank_kvs.RX < -15) {
    Serial.println("leftTranslation");
    left_sw = true;
    leftTranslation();
  } else if (tank_kvs.RX > 15) {
    Serial.println("rightTranslation");
    right_sw = true;
    rightTranslation();
  } else {
    clearSw();
    // Serial.println("stop");
    stop();
  }
}

void MotorControl::clearSw() {
  left_sw = false;
  right_sw = false;
}

// 前进
void MotorControl::forward() {
  motor_a(LOW, HIGH);
  motor_b(LOW, HIGH);
  motor_c(HIGH, LOW);
  motor_d(HIGH, LOW);
}

// 后退
void MotorControl::backward() {
  motor_a(HIGH, LOW);
  motor_b(HIGH, LOW);
  motor_c(LOW, HIGH);
  motor_d(LOW, HIGH);
}

// 左旋转
void MotorControl::leftRotate() {
  motor_a(HIGH, LOW);
  motor_b(HIGH, LOW);
  motor_c(HIGH, LOW);
  motor_d(HIGH, LOW);
}

// 右旋转
void MotorControl::rightRotate() {
  motor_a(LOW, HIGH);
  motor_b(LOW, HIGH);
  motor_c(LOW, HIGH);
  motor_d(LOW, HIGH);
}

// 左平移
void MotorControl::leftTranslation() {
  motor_a(LOW, HIGH);
  motor_b(HIGH, LOW);
  motor_c(LOW, HIGH);
  motor_d(HIGH, LOW);
}

// 右平移
void MotorControl::rightTranslation() {
  motor_a(HIGH, LOW);
  motor_b(LOW, HIGH);
  motor_c(HIGH, LOW);
  motor_d(LOW, HIGH);
}

// 停止
void MotorControl::stop() {
  motor_a(LOW, LOW);
  motor_b(LOW, LOW);
  motor_c(LOW, LOW);
  motor_d(LOW, LOW);
}

// 刹车
void MotorControl::brake() {
  motor_a(HIGH, HIGH);
  motor_b(HIGH, HIGH);
  motor_c(HIGH, HIGH);
  motor_d(HIGH, HIGH);
}

// 控制电机a
void MotorControl::motor_a(int in1, int in2) {
  digitalWrite(MOTOR_A1_PIN, in1);
  digitalWrite(MOTOR_A2_PIN, in2);
}

// 控制电机b
void MotorControl::motor_b(int in1, int in2) {
  digitalWrite(MOTOR_B1_PIN, in1);
  digitalWrite(MOTOR_B2_PIN, in2);
}

// 控制电机c
void MotorControl::motor_c(int in1, int in2) {
  digitalWrite(MOTOR_C1_PIN, in1);
  digitalWrite(MOTOR_C2_PIN, in2);
}

// 控制电机d
void MotorControl::motor_d(int in1, int in2) {
  digitalWrite(MOTOR_D1_PIN, in1);
  digitalWrite(MOTOR_D2_PIN, in2);
}